/*
 * ThreadMotorController.cpp
 *
 *  Created on: Jan 26, 2013
 *      Author: Drew Prevost
 */

#include "ThreadMotorController.h"

ThreadMotorController::ThreadMotorController( int nPin )
{
  this->nPin = nPin;
  this->nCurSpeed = 0;
  this->nTargetSpeed = 0;
  this->nAccRateInMicrosPerAcc = 0;
}

ThreadMotorController::~ThreadMotorController()
{
}

int ThreadMotorController::getSpeed()
{
  return nCurSpeed;
}

void ThreadMotorController::setSpeed(
                                      int nTargetSpeed,
                                      unsigned long nAccRateInMicrosPerAcc )
{
  this->nTargetSpeed = nTargetSpeed;
  this->nAccRateInMicrosPerAcc = nAccRateInMicrosPerAcc;
}

#define __MOTOR_POS_NEXT( nDiffFromTarget) (nCurSpeed) + ( ((nDiffFromTarget) > 0) ? 1 : -1 )
#define __MOTOR_DELAY_MICROS() ( ( (nAccRateInMicrosPerAcc) > 0 ) ? (nAccRateInMicrosPerAcc) : 0 )
#define __MOTOR_ELAPSED_TIME() ( micros() - nAccRateInMicrosPerAcc )
#define __MOTOR_DELAY_MICROS_LESS_ELAPSED_TIME() ( (__MOTOR_DELAY_MICROS() > __MOTOR_ELAPSED_TIME()) ? (__MOTOR_DELAY_MICROS() - __MOTOR_ELAPSED_TIME()) : 0  )
bool ThreadMotorController::run()
{
  threadMicroDelay.run();

  int nDiff = nTargetSpeed - nCurSpeed;

  PT_BEGIN( )
  ;

  if ( nDiff != 0 )
  {
    analogWrite( nPin, __MOTOR_POS_NEXT( nDiff) );
    nCurSpeed = __MOTOR_POS_NEXT( nDiff);
    if ( ( nAccRateInMicrosPerAcc > 0 ) && ( nTargetSpeed != nCurSpeed ) )
    {
      threadMicroDelay.setDelay( __MOTOR_DELAY_MICROS_LESS_ELAPSED_TIME() );
      PT_SPAWN( threadMicroDelay );
    }
  }

PT_END()
;
return false;
}
